# Mission 7 complete
# With abstraction from modified Obj. 11
# With all three extensions
from botcore import *
from time import sleep

power = 1
thresh = 75
go_motors = False
speed = 40

def cal_thresh():
    global thresh
    det = 101
    sensed = prox.range(10, power)

    if sensed[LEFT] > 0:
        det = sensed[LEFT]
    if sensed[RIGHT] > 0:
        det = min(det, sensed[RIGHT])

    if det > 100:
        thresh = 100
    else:
        thresh = det - 5

    print("Sensed=", sensed, " set thresh=", thresh)

def cal_power():
    global power
    power = 1
    while power < 9:
        cal_thresh()
        if thresh < 100:
            break
        power = power + 1

def drive(left_speed, right_speed):
    motors.run(LEFT, left_speed)
    motors.run(RIGHT, right_speed)

def toggle_motors():
    global go_motors
    go_motors = not go_motors
    motors.enable(go_motors)
    leds.user_num(0, go_motors)

# -- Main program --      
while True:
    if buttons.was_pressed(1):
        sleep(0.1)
        buttons.was_pressed(1)
        cal_power()
        print("Power=", power, "thresh=", thresh)

    if buttons.was_pressed(0):
        sleep(0.1)
        buttons.was_pressed(0)
        toggle_motors()
    
    p = prox.detect(power, thresh)
    leds.prox(p)

    if p[LEFT] and p[RIGHT]:
        drive(speed, speed)
    elif p[LEFT]:
        # Integer division // ensures
        # the result is an integer
        drive(0, speed//2)
    elif p[RIGHT]:
        drive(speed//2, 0)
    else:
        drive(0, 0)